#!/usr/bin/env python3

import os
import launch
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # Declare the use_sim_time parameter
    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation (Gazebo) clock if true'
    )

    # Get package directories
    my_webots_sim_dir = get_package_share_directory('my_webots_sim')
    my_robot_nav_dir = get_package_share_directory('my_robot_nav')
    carto_slam = LaunchConfiguration('carto_slam', default='true')

    # Load IMU configuration
    imu_config = os.path.join(my_robot_nav_dir, 'config', 'imu.yaml')

    # Include the robot launch file
    robot_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(my_webots_sim_dir, 'launch', 'robot_launch.py')
        ),
        launch_arguments={
            'use_sim_time': LaunchConfiguration('use_sim_time')
        }.items()
    )

    # Include the EKF launch file
    robot_ekf = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(my_robot_nav_dir, 'launch', 'wheeltec_ekf.launch.py')
        ),
        launch_arguments={
            'carto_slam': carto_slam,
            'use_sim_time': LaunchConfiguration('use_sim_time')
        }.items()
    )
    
    imu_filter_node = Node(
        package='imu_filter_madgwick',
        executable='imu_filter_madgwick_node',
        parameters=[imu_config],
        remappings=[
            ('/imu/data_raw', '/imu')
        ],
        ros_arguments=['--ros-args', '--log-level', 'warn']
    )

    return LaunchDescription([
        use_sim_time_arg,
        # robot_launch,
        imu_filter_node,
        robot_ekf
    ]) 